# include <cstdlib>
# include <iostream>
# include <iomanip>
# include <cmath>
# include <fstream>
# include <cstring>

using namespace std;

# include "tet.hh"
# include "utils.hh"

//****************************************************************************
//
//  Purpose:
//
//    BASIS_MN_TET4: all bases at N points for a TET4 element.
//
//  Discussion:
//
//    The routine is given the coordinates of the vertices of a tetrahedron.
//
//    It works directly with these coordinates, and does not refer to a
//    reference element.
//
//  Reference:
//
//    Olgierd Zienkiewicz,
//    The Finite Element Method,
//    Sixth Edition,
//    Butterworth-Heinemann, 2005,
//    ISBN: 0750663200,
//    LC: TA640.2.Z54.
//
//  Parameters:
//
//    Input, double T[3*4], the coordinates of the vertices.
//
//    Input, int N, the number of evaluation points.
//
//    Input, double P[3*N], the points where the basis functions
//    are to be evaluated.
//
//    Output, double PHI[4*N], the value of the basis functions
//    at the evaluation points.
//
double* Tet4::evaluate_basis ( int n, const double p[])
{
  double *phi = new double[4*n];
  const double *t = this->vertices;

  int j;

//
//             | xp x2 x3 x4 |
//  Phi(1,P) = | yp y2 y3 y4 | / volume
//             | zp z2 z3 z4 |
//             |  1  1  1  1 |
//
  for ( j = 0; j < n; j++ )
  {
    phi[0+j*4] = (
        p[0+j*3] * (
          t[1+1*3] * ( t[2+2*3] - t[2+3*3] )
        - t[1+2*3] * ( t[2+1*3] - t[2+3*3] )
        + t[1+3*3] * ( t[2+1*3] - t[2+2*3] ) )
      - t[0+1*3] * (
          p[1+j*3] * ( t[2+2*3] - t[2+3*3] )
        - t[1+2*3] * ( p[2+j*3] - t[2+3*3] )
        + t[1+3*3] * ( p[2+j*3] - t[2+2*3] ) )
      + t[0+2*3] * (
          p[1+j*3] * ( t[2+1*3] - t[2+3*3] )
        - t[1+1*3] * ( p[2+j*3] - t[2+3*3] )
        + t[1+3*3] * ( p[2+j*3] - t[2+1*3] ) )
      - t[0+3*3] * (
          p[1+j*3] * ( t[2+1*3] - t[2+2*3] )
        - t[1+1*3] * ( p[2+j*3] - t[2+2*3] )
        + t[1+2*3] * ( p[2+j*3] - t[2+1*3] ) ) ) / volume;
//
//             | x1 xp x3 x4 |
//  Phi(2,P) = | y1 yp y3 y4 | / volume
//             | z1 zp z3 z4 |
//             |  1  1  1  1 |
//
    phi[1+j*4] = (
        t[0+0*3] * (
          p[1+j*3] * ( t[2+2*3] - t[2+3*3] )
        - t[1+2*3] * ( p[2+j*3] - t[2+3*3] )
        + t[1+3*3] * ( p[2+j*3] - t[2+2*3] ) )
      - p[0+j*3]   * (
          t[1+0*3] * ( t[2+2*3] - t[2+3*3] )
        - t[1+2*3] * ( t[2+0*3] - t[2+3*3] )
        + t[1+3*3] * ( t[2+0*3] - t[2+2*3] ) )
      + t[0+2*3] * (
          t[1+0*3] * ( p[2+j*3] - t[2+3*3] )
        - p[1+j*3] * ( t[2+0*3] - t[2+3*3] )
        + t[1+3*3] * ( t[2+0*3] - p[2+j*3] ) )
      - t[0+3*3] * (
          t[1+0*3] * ( p[2+j*3] - t[2+2*3] )
        - p[1+j*3] * ( t[2+0*3] - t[2+2*3] )
        + t[1+2*3] * ( t[2+0*3] - p[2+j*3] ) ) ) / volume;
//
//             | x1 x2 xp x4 |
//  Phi(3,P) = | y1 y2 yp y4 | / volume
//             | z1 z2 zp z4 |
//             |  1  1  1  1 |
//
    phi[2+j*4] = (
        t[0+0*3] * (
          t[1+1*3] * ( p[2+j*3] - t[2+3*3] )
        - p[1+j*3] * ( t[2+1*3] - t[2+3*3] )
        + t[1+3*3] * ( t[2+1*3] - p[2+j*3] ) )
      - t[0+1*3] * (
          t[1+0*3] * ( p[2+j*3] - t[2+3*3] )
        - p[1+j*3] * ( t[2+0*3] - t[2+3*3] )
        + t[1+3*3] * ( t[2+0*3] - p[2+j*3] ) )
      + p[0+j*3] * (
          t[1+0*3] * ( t[2+1*3] - t[2+3*3] )
        - t[1+1*3] * ( t[2+0*3] - t[2+3*3] )
        + t[1+3*3] * ( t[2+0*3] - t[2+1*3] ) )
      - t[0+3*3] * (
          t[1+0*3] * ( t[2+1*3] - p[2+j*3] )
        - t[1+1*3] * ( t[2+0*3] - p[2+j*3] )
        + p[1+j*3] * ( t[2+0*3] - t[2+1*3] ) ) ) / volume;
//
//             | x1 x2 x3 xp |
//  Phi(4,P) = | y1 y2 y3 yp | / volume
//             | z1 z2 z3 zp |
//             |  1  1  1  1 |
//
    phi[3+j*4] = (
        t[0+0*3] * (
          t[1+1*3] * ( t[2+2*3] - p[2+j*3] )
        - t[1+2*3] * ( t[2+1*3] - p[2+j*3] )
        + p[1+j*3] * ( t[2+1*3] - t[2+2*3] ) )
      - t[0+1*3] * (
          t[1+0*3] * ( t[2+2*3] - p[2+j*3] )
        - t[1+2*3] * ( t[2+0*3] - p[2+j*3] )
        + p[1+j*3] * ( t[2+0*3] - t[2+2*3] ) )
      + t[0+2*3] * (
          t[1+0*3] * ( t[2+1*3] - p[2+j*3] )
        - t[1+1*3] * ( t[2+0*3] - p[2+j*3] )
        + p[1+j*3] * ( t[2+0*3] - t[2+1*3] ) )
      - p[0+j*3] * (
          t[1+0*3] * ( t[2+1*3] - t[2+2*3] )
        - t[1+1*3] * ( t[2+0*3] - t[2+2*3] )
        + t[1+2*3] * ( t[2+0*3] - t[2+1*3] ) ) ) / volume;
  }
  return phi;
}
//****************************************************************************

int Tet4::test ( )
{
# define NODE_NUM 4

  double *c;
  double c_sum;
  int i;
  int j;
  double *p;
  //double phi1[NODE_NUM*1];
  double phi1_sum;
  //double phi4[NODE_NUM*NODE_NUM];
  int seed=12345678;
  double *t;
  int test;
  int test_num = 5;

  cout << "\n";
  cout << "BASIS_MN_TET4_TEST:\n";
  cout << "  Verify basis functions for element TET4.\n";
  cout << "\n";
  cout << "  Number of nodes = " << NODE_NUM << "\n";

  t = r8mat_uniform_01_new ( 3, 4, &seed );

  cout << "\n";
  cout << "  Tetrahedron Nodes:\n";
  cout << "\n";
  for ( j = 0; j < NODE_NUM; j++ )
  {
    cout << "  "
         << setw(10) << t[0+j*3] << "  "
         << setw(10) << t[1+j*3] << "\n";
  }

  cout << "\n";
  cout << "  The basis function values at basis nodes\n";
  cout << "  should form the identity matrix.\n";
  cout << "\n";

  Tet4 lintet(t);
  double* phi4 = lintet.evaluate_basis ( NODE_NUM, t);

  for ( j = 0; j < NODE_NUM; j++ )
  {
    for ( i = 0; i < NODE_NUM; i++ )
    {
      cout << "  " << fixed << setprecision(4) << setw(10) << phi4[i+j*NODE_NUM];
    }
    cout << "\n";
  }
  delete phi4;

  cout << "\n";
  cout << "  The basis function values at ANY point P\n";
  cout << "  should sum to 1:\n";
  cout << "\n";
  cout << "    ------------P-------------    "
       << "-----------------PHI----------------   PHI_SUM\n";
  cout << "\n";

  for ( test = 1; test <= test_num; test++ )
  {
    c = r8vec_uniform_01_new ( 4, &seed );

    c_sum = r8vec_sum ( 4, c );
    for ( i = 0; i < 4; i++ )
    {
      c[i] = c[i] / c_sum;
    }
    p = r8mat_mv ( 3, 4, t, c );

    Tet4 lintet2(t);
    double* phi1 = lintet2.evaluate_basis ( 1, p);

    phi1_sum = r8vec_sum ( NODE_NUM, phi1 );

    cout << "  " << setw(8) << p[0]
         << "  " << setw(8) << p[1]
         << "  " << setw(8) << p[2]
         << "  " << setw(8) << phi1[0]
         << "  " << setw(8) << phi1[1]
         << "  " << setw(8) << phi1[2]
         << "  " << setw(8) << phi1[3]
         << "  " << setw(8) << phi1_sum << "\n";

    delete phi1;
    delete [] c;
    delete [] p;
  }

  delete [] t;

  return 0;
# undef NODE_NUM
}
//****************************************************************************


//****************************************************************************
//
//  Purpose:
//
//    BASIS_MN_TET10: all bases at N points for a TET10 element.
//
//  Discussion:
//
//    The routine is given the coordinates of the vertices of a tetrahedron.
//
//    It works directly with these coordinates, and does not refer to a
//    reference element.
//
//    P1 through P4 are vertices.
//
//    P1 <= P5  <= P2
//    P2 <= P6  <= P3
//    P1 <= P7  <= P3
//    P1 <= P8  <= P4
//    P2 <= P9  <= P4
//    P3 <= P10 <= P4
//
//  Reference:
//
//    Olgierd Zienkiewicz,
//    The Finite Element Method,
//    Sixth Edition,
//    Butterworth-Heinemann, 2005,
//    ISBN: 0750663200,
//    LC: TA640.2.Z54.
//
//  Parameters:
//
//    Input, double T[3*4], the coordinates of the vertices.
//
//    Input, int N, the number of evaluation points.
//
//    Input, double P[3*N], the points where the basis functions
//    are to be evaluated.
//
//    Output, double PHI[10*N], the value of the basis functions
//    at the evaluation points.
//
double* Tet10::evaluate_basis ( int n, const double p[] )
{
  int j;
  double *phi = new double[10*n];

  Tet4 lintet(this->vertices);

  double *phi_linear = lintet.evaluate_basis (n, p);

  for ( j = 0; j < n; j++ )
  {
    phi[0+j*10] = ( 2.0 * phi_linear[0+j*4]  - 1.0 ) * phi_linear[0+j*4];
    phi[1+j*10] = ( 2.0 * phi_linear[1+j*4]  - 1.0 ) * phi_linear[1+j*4];
    phi[2+j*10] = ( 2.0 * phi_linear[2+j*4]  - 1.0 ) * phi_linear[2+j*4];
    phi[3+j*10] = ( 2.0 * phi_linear[3+j*4]  - 1.0 ) * phi_linear[3+j*4];
    phi[4+j*10] =   4.0 * phi_linear[0+j*4]          * phi_linear[1+j*4];
    phi[5+j*10] =   4.0 * phi_linear[1+j*4]          * phi_linear[2+j*4];
    phi[6+j*10] =   4.0 * phi_linear[0+j*4]          * phi_linear[2+j*4];
    phi[7+j*10] =   4.0 * phi_linear[0+j*4]          * phi_linear[3+j*4];
    phi[8+j*10] =   4.0 * phi_linear[1+j*4]          * phi_linear[3+j*4];
    phi[9+j*10] =   4.0 * phi_linear[2+j*4]          * phi_linear[3+j*4];
  }
  delete phi_linear;

  return phi;
}
//****************************************************************************

int Tet10::test ( )
{
  double *c;
  double c_sum;
  int i;
  int j;
  double *p;
  double p10[3*10];
  //double phi1[10*1];
  double phi1_sum;
  //double phi10[10*10];
  int seed=12345678;
  double *t;
  int test;
  int test_num = 5;

  cout << "\n";
  cout << "BASIS_MN_TET10_TEST:\n";
  cout << "  Verify basis functions for element TET10.\n";
  cout << "\n";
  cout << "  Number of nodes = 10.\n";

  t = r8mat_uniform_01_new ( 3, 4, &seed );

  cout << "\n";
  cout << "  Tetrahedron Nodes:\n";
  cout << "\n";
  for ( j = 0; j < 4; j++ )
  {
    cout << "  " << setw(8) << j
         << "  " << setw(14) << t[0+j*3]
         << "  " << setw(14) << t[1+j*3]
         << "  " << setw(14) << t[2+j*3] << "\n";
  }

  cout << "\n";
  cout << "  The basis function values at basis nodes\n";
  cout << "  should form the identity matrix.\n";
  cout << "\n";

  for ( i = 0; i < 3; i++ )
  {
    p10[i+0*3] = t[i+0*3];
    p10[i+1*3] = t[i+1*3];
    p10[i+2*3] = t[i+2*3];
    p10[i+3*3] = t[i+3*3];
    p10[i+4*3] = 0.5 * ( t[i+0*3] + t[i+1*3] );
    p10[i+5*3] = 0.5 * ( t[i+1*3] + t[i+2*3] );
    p10[i+6*3] = 0.5 * ( t[i+0*3] + t[i+2*3] );
    p10[i+7*3] = 0.5 * ( t[i+0*3] + t[i+3*3] );
    p10[i+8*3] = 0.5 * ( t[i+1*3] + t[i+3*3] );
    p10[i+9*3] = 0.5 * ( t[i+2*3] + t[i+3*3] );
  }

  Tet10 quadtet(t);
  double* phi10 = quadtet.evaluate_basis ( 10, p10);

  for ( i = 0; i < 10; i++ )
  {
    for ( j = 0; j < 10; j++ )
    {
      cout << "  " << fixed << setprecision(4) << setw(7) << phi10[i+j*10];
    }
    cout << "\n";
  }
  delete phi10;

  cout << "\n";
  cout << "  The basis function values at ANY point P\n";
  cout << "  should sum to 1:\n";
  cout << "\n";
  cout << "    ------------P-------------    ";
  cout << "----------------------------------------------------";
  cout << "PHI-----------------------------------------   PHI_SUM\n";
  cout << "\n";

  for ( test = 1; test <= test_num; test++ )
  {
    c = r8vec_uniform_01_new ( 4, &seed );

    c_sum = r8vec_sum ( 4, c );
    for ( i = 0; i < 4; i++ )
    {
      c[i] = c[i] / c_sum;
    }
    p = r8mat_mv ( 3, 4, t, c );
    
    Tet10 quadtet2(t);
    double* phi1 = quadtet2.evaluate_basis ( 1, p);

    //basis_mn_tet10 ( t, 1, p, phi1 );
    phi1_sum = r8vec_sum ( 10, phi1 );

    cout << "  " << setw(8) << p[0]
         << "  " << setw(8) << p[1]
         << "  " << setw(8) << p[2]
         << "  " << setw(8) << phi1[0]
         << "  " << setw(8) << phi1[1]
         << "  " << setw(8) << phi1[2]
         << "  " << setw(8) << phi1[3]
         << "  " << setw(8) << phi1[4]
         << "  " << setw(8) << phi1[5]
         << "  " << setw(8) << phi1[6]
         << "  " << setw(8) << phi1[7]
         << "  " << setw(8) << phi1[8]
         << "  " << setw(8) << phi1[9]
         << "  " << setw(8) << phi1_sum << "\n";

    delete phi1;
    delete [] c;
    delete [] p;
  }

  delete [] t;

  return 0;
}
//****************************************************************************80


double* Tet::physical_to_reference_coords ( int n, double phy[] )

//****************************************************************************80
//
//  Purpose:
//
//    PHYSICAL_TO_REFERENCE_TET4 maps physical points to reference points.
//
//  Discussion:
//
//    Given the vertices of an order 4 physical tetrahedron and a point
//    (X,Y,Z) in the physical tetrahedron, the routine computes the value
//    of the corresponding point (R,S,T) in the reference tetrahedron.
//
//  Licensing:
//
//    This code is distributed under the GNU LGPL license.
//
//  Modified:
//
//    10 August 2009
//
//  Author:
//
//    John Burkardt
//
//  Parameters:
//
//    Input, double T[3*4], the coordinates of the vertices of the
//    physical tetrahedron.  The vertices are assumed to be the images of
//    (1,0,0), (0,1,0), (0,0,1) and (0,0,0) respectively.
//
//    Input, int N, the number of points to transform.
//
//    Input, double PHY[3*N], the coordinates of physical points
//    to be transformed.
//
//    Output, double PHYSICAL_TO_REFERENCE[3*N], the coordinates of the
//    corresponding points in the reference tetrahedron.
//
{
  const double* t = this->vertices;
  double a[3*3];
  int i;
  int j;
  double *ref;

  for ( j = 0; j < 3; j++ )
  {
    for ( i = 0; i < 3; i++ )
    {
      a[i+j*3] = t[i+j*3] - t[i+3*3];
    }
  }

  ref = new double[3*n];

  for ( j = 0; j < n; j++ )
  {
    for ( i = 0; i < 3; i++ )
    {
      ref[i+j*3] = phy[i+j*3] - t[i+3*3];
    }
  }

  r8ge_fss ( 3, a, n, ref );

  return ref;
}
//****************************************************************************80


double* Tet::reference_to_physical_coords ( int n, double ref[] )

//****************************************************************************80
//
//  Purpose:
//
//    REFERENCE_TO_PHYSICAL_TET4 maps TET4 reference points to physical points.
//
//  Discussion:
//
//    Given the vertices of an order 4 physical tetrahedron and a point
//    (R,S,T) in the reference tetrahedron, the routine computes the value
//    of the corresponding image point (X,Y,Z) in physical space.
//
//  Licensing:
//
//    This code is distributed under the GNU LGPL license.
//
//  Modified:
//
//    10 August 2009
//
//  Author:
//
//    John Burkardt
//
//  Parameters:
//
//    Input, double T[3*4], the coordinates of the vertices.
//    The vertices are assumed to be the images of (1,0,0), (0,1,0),
//    (0,0,1) and (0,0,0) respectively.
//
//    Input, int N, the number of points to transform.
//
//    Input, double REF[3*N], points in the reference element.
//
//    Output, double REFERENCE_TO_PHYSICAL_TET4[3*N], corresponding points in the
//    physical element.
//
{
  int i;
  int j;
  double *phy;
  const double* t = this->vertices;

  phy = new double[3*n];

  for ( j = 0; j < n; j++ )
  {
    for ( i = 0; i < 3; i++ )
    {
      phy[i+j*3] =
          t[i+0*3] *         ref[0+j*3]
        + t[i+1*3] *                      ref[1+j*3]
        + t[i+2*3] *                                   ref[2+j*3]
        + t[i+3*3] * ( 1.0 - ref[0+j*3] - ref[1+j*3] - ref[2+j*3] );
    }
  }

  return phy;
}
//****************************************************************************80

double* Tet::barycentric_coords ( double p[3] )

//****************************************************************************80
//
//  Purpose:
//
//    TETRAHEDRON_BARYCENTRIC returns the barycentric coordinates of a point.
//
//  Discussion:
//
//    The barycentric coordinates of a point P with respect to
//    a tetrahedron are a set of four values C(1:4), each associated
//    with a vertex of the tetrahedron.  The values must sum to 1.
//    If all the values are between 0 and 1, the point is contained
//    within the tetrahedron.
//
//    The barycentric coordinate of point X related to vertex A can be
//    interpreted as the ratio of the volume of the tetrahedron with
//    vertex A replaced by vertex X to the volume of the original
//    tetrahedron.
//
//  Licensing:
//
//    This code is distributed under the GNU LGPL license.
//
//  Modified:
//
//    12 August 2005
//
//  Author:
//
//    John Burkardt
//
//  Parameters:
//
//    Input, double TETRA[3*4], the vertices of the tetrahedron.
//
//    Input, double P[3], the point to be checked.
//
//    Output, double C[4], the barycentric coordinates of the point with
//    respect to the tetrahedron.
//
{
# define N 3
# define RHS_NUM 1

  double a[N*(N+RHS_NUM)];
  double *c;
  int info;
  const double* tetra = this->vertices;
//
//  Set up the linear system
//
//    ( X2-X1  X3-X1  X4-X1 ) C1    X - X1
//    ( Y2-Y1  Y3-Y1  Y4-Y1 ) C2  = Y - Y1
//    ( Z2-Z1  Z3-Z1  Z4-Z1 ) C3    Z - Z1
//
//  which is satisfied by the barycentric coordinates.
//

  a[0+0*N] = tetra[0+1*3] - tetra[0+0*3];
  a[1+0*N] = tetra[1+1*3] - tetra[1+0*3];
  a[2+0*N] = tetra[2+1*3] - tetra[2+0*3];

  a[0+1*N] = tetra[0+2*3] - tetra[0+0*3];
  a[1+1*N] = tetra[1+2*3] - tetra[1+0*3];
  a[2+1*N] = tetra[2+2*3] - tetra[2+0*3];

  a[0+2*N] = tetra[0+3*3] - tetra[0+0*3];
  a[1+2*N] = tetra[1+3*3] - tetra[1+0*3];
  a[2+2*N] = tetra[2+3*3] - tetra[2+0*3];

  a[0+3*N] = p[0]         - tetra[0+0*3];
  a[1+3*N] = p[1]         - tetra[1+0*3];
  a[2+3*N] = p[2]         - tetra[2+0*3];
//
//  Solve the linear system.
//
  info = r8mat_solve ( N, RHS_NUM, a );

  if ( info != 0 )
  {
    cout << "\n";
    cout << "TETRAHEDRON_BARYCENTRIC - Fatal error!\n";
    cout << "  The linear system is singular.\n";
    cout << "  The input data does not form a proper tetrahedron.\n";
    exit ( 1 );
  }

  c = new double[4];

  c[1] = a[0+3*N];
  c[2] = a[1+3*N];
  c[3] = a[2+3*N];

  c[0] = 1.0 - c[1] - c[2] - c[3];

  return c;
# undef N
# undef RHS_NUM
}


